#include "ImgRegistration.h"


CImgRegistration::CImgRegistration(void):m_iNumFeaturePts(8),m_iNumNeighborSize(15),m_pImg(NULL),m_pScaledImg(NULL),m_fScaleFactor(0.25f),
	m_fThresholdRot(0.001f), m_bTranslationOnly(false)
{
	m_pImg = new COpenCVImg();
	m_pScaledImg = new COpenCVImg();
}


CImgRegistration::~CImgRegistration(void)
{
	delete(m_pImg);
	delete(m_pScaledImg);
}

void CImgRegistration::LoadImageForDisplay(char* strFileName, bool bColor, float fScaleFactor)
{
	m_pImg->loadImg(strFileName, bColor);
	//scaled img for displaying
	m_fScaleFactor = fScaleFactor;
	
	m_pScaledImg->CreateScaledImg(m_pImg,m_fScaleFactor);
}

/*******************************************************************/
// affine transformation A*P = P'
//  
//  | cos(theta)  -sin(theta)  t_x | | x |     | x' |
//  | sin(theta)   cos(theta)  t_y | | y |  =  | y' |
//  |	 0				0		1  | | 1 |	   | 1  |
//   
//  Denote cos(theta) as m, sin(theta) as n:
//  m*x + n*(-y) + t_x*1 = x' and  n*x + m*y + t_y*1 = y'
//
//  | x1  -y1  1 | | m |     | x1' |
//  | x2  -y2  1 | | n |  =  | x2' |
//  | x3  -y3  1 | | t_x |	 | x3' |
//  and
//  | x1  y1  1 | | n |     | y1' |
//  | x2  y2  1 | | m |  =  | y2' |
//  | x3  y3  1 | | t_y |   | y3' |
//
//   X = ( A^(T)*A )^(-1) A^(T) b
/*******************************************************************/
//Equal to CvGetAffineTransform()???
void CImgRegistration::ComputeAffineTransform(CImgRegistration* img1)
{
	int iNumElem1 = m_arrCorners.size();
	int iNumElem2 = img1->m_arrCorners.size();
	if(iNumElem1<=0 || iNumElem2<=0 || (iNumElem1!=iNumElem2) )
		return;

	//use 8 pts for over-determined matrix
	MatrixXf A1(iNumElem1,3), A2(iNumElem1,3);
	MatrixXf b1(iNumElem1,1), b2(iNumElem1,1);

	//fill the data
	for (int i = 0; i < img1->m_arrCorners.size(); i++)
	{
		A1(i,0) = img1->m_arrCorners[i].x;
		A1(i,1) = -img1->m_arrCorners[i].y;
		A1(i,2) = 1.0;
		b1(i,0) = m_arrCorners[i].x;

		A2(i,0) = img1->m_arrCorners[i].x;
		A2(i,1) = img1->m_arrCorners[i].y;
		A2(i,2) = 1.0;
		b2(i,0) = m_arrCorners[i].y;
	}

	//solve first linear group
	//X = ( A^(T)*A )^(-1) A^(T) b
	MatrixXf X1(3,1);
	MatrixXf tmp(3,3), tmpb(3,1);
	tmp = A1.transpose()*A1;
	tmp = tmp.inverse();
	tmpb = A1.transpose()*b1;
	X1 = tmp * tmpb;
	printf("m = %f, n = %f, t_x = %f\n", X1(0,0),-X1(1,0),X1(2,0));
	m_2DTransform.m_fCosTheta = X1(0,0);
	m_2DTransform.m_fSinTheta = -X1(1,0);
	m_2DTransform.m_fTransX = X1(2,0);

	if( (fabs(m_2DTransform.m_fCosTheta-1.0f) < m_fThresholdRot ) && (fabs(m_2DTransform.m_fSinTheta-0.0f) < m_fThresholdRot) )
	{
		m_2DTransform.m_fCosTheta = 1.0f;
		m_2DTransform.m_fSinTheta = 0.0f;
		m_bTranslationOnly = true;
	}
	//solve second linear group
	MatrixXf X2(3,1);
	tmp = A2.transpose()*A2;
	tmp = tmp.inverse();
	tmpb = A2.transpose()*b2;
	X2 = tmp * tmpb;
	printf("n = %f, m = %f, t_y = %f\n", X2(0,0),X2(1,0),X2(2,0));
	m_2DTransform.m_fTransY = X2(2,0);
}

void CImgRegistration::SaveFeaturePts(char* strFileName)
{
	if(strFileName == NULL || m_arrCorners.size() == 0)
		 return;

	FILE* fp = fopen(strFileName, "w");
	fprintf(fp,"%d\n", m_arrCorners.size());
	for (int i = 0; i < m_arrCorners.size(); i++)
	{
		CvPoint2D32f pt = m_arrCorners[i];
		fprintf(fp, "%f %f\n", pt.x, pt.y);
	}
	fclose(fp);
}

